Conversation
WalkthroughThis change modifies the SimRobot simulator to synchronize simulation timing with DiffDriveService (0.02s period), cache noisy velocity values for consistent encoder/IMU readback, implement first-tick initialization, apply emergency-stop zero-velocity forcing, and replace distance-based charging detection with hysteresis thresholds and position snapping. Changes
Estimated code review effort🎯 3 (Moderate) | ⏱️ ~22 minutes Poem
🚥 Pre-merge checks | ✅ 3 | ❌ 2❌ Failed checks (1 warning, 1 inconclusive)
✅ Passed checks (3 passed)
✏️ Tip: You can configure your own custom pre-merge checks in the settings. ✨ Finishing Touches📝 Generate docstrings
🧪 Generate unit tests (beta)
Thanks for using CodeRabbit! It's free for OSS, and your support helps us grow. If you like it, consider giving us a shout-out. Comment |
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
There was a problem hiding this comment.
Actionable comments posted: 1
Caution
Some comments are outside the diff and can’t be posted inline due to platform limitations.
⚠️ Outside diff range comments (1)
src/mower_simulation/src/SimRobot.cpp (1)
153-165:⚠️ Potential issue | 🟠 MajorMajor: 25 mm threshold + noise-integrated pose can cause
is_charging_to oscillate while docked.Because
pos_x_/pos_y_are now integrated withnoisy_vx/noisy_vr(rather than rawvx_/vr_), a stationary robot (command v=ω=0) performs a random walk of roughlyσ ≈ 0.02·sqrt(1/dt)·dt ≈ 2.8 mm/sper second of wall time, growing as √t. Over tens of seconds idling on the dock, distance-to-dock can wander past 25 mm, flipping into theelsebranch and logging "Stopped Charging" → then back to "Charging" on the next step.PowerService::tick()forwards this intocharge_volts/charging_status, whichDockingBehavior/IdleBehaviorwatch for state transitions — so this can surface as flapping charging state in the simulated stack.Consider either (a) adding hysteresis on the dock check, (b) keeping integration zero-locked when the commanded twist is zero, or (c) comparing against a low-pass-filtered pose.
🛡️ One possible fix — hysteresis on the dock distance
- // Update Charger Status - if (sqrt((docking_pos_x_ - pos_x_) * (docking_pos_x_ - pos_x_) + - (docking_pos_y_ - pos_y_) * (docking_pos_y_ - pos_y_)) < 0.025) { - if (!is_charging_) { - spdlog::info("Charging"); - is_charging_ = true; - charging_started_time = ros::Time::now(); - } - } else { - if (is_charging_) { - spdlog::info("Stopped Charging"); - is_charging_ = false; - } - } + // Update Charger Status (with hysteresis so noise-driven pose drift while + // parked on the dock doesn't flap charging state). + const double dock_dist = std::hypot(docking_pos_x_ - pos_x_, docking_pos_y_ - pos_y_); + constexpr double kDockEnterDist = 0.025; + constexpr double kDockExitDist = 0.10; + if (!is_charging_ && dock_dist < kDockEnterDist) { + spdlog::info("Charging"); + is_charging_ = true; + charging_started_time = ros::Time::now(); + } else if (is_charging_ && dock_dist > kDockExitDist) { + spdlog::info("Stopped Charging"); + is_charging_ = false; + }🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/mower_simulation/src/SimRobot.cpp` around lines 153 - 165, The dock-distance check in SimRobot.cpp that toggles is_charging_ based on a 25 mm radius is causing oscillation because pos_x_/pos_y_ include noisy integrated motion; modify the dock check to add hysteresis (e.g., enter charging when distance < 0.02 m and exit when distance > 0.03 m) around the existing block that reads docking_pos_x_, docking_pos_y_, pos_x_, pos_y_, is_charging_, and charging_started_time, or alternatively prevent integrating noisy_vx/noisy_vr into pos_x_/pos_y_ when the commanded twist is zero (zero-locking) so the robot stays stable while docked—pick one approach and adjust the condition/logic around the sqrt distance test and the is_charging_ toggles accordingly.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Outside diff comments:
In `@src/mower_simulation/src/SimRobot.cpp`:
- Around line 153-165: The dock-distance check in SimRobot.cpp that toggles
is_charging_ based on a 25 mm radius is causing oscillation because
pos_x_/pos_y_ include noisy integrated motion; modify the dock check to add
hysteresis (e.g., enter charging when distance < 0.02 m and exit when distance >
0.03 m) around the existing block that reads docking_pos_x_, docking_pos_y_,
pos_x_, pos_y_, is_charging_, and charging_started_time, or alternatively
prevent integrating noisy_vx/noisy_vr into pos_x_/pos_y_ when the commanded
twist is zero (zero-locking) so the robot stays stable while docked—pick one
approach and adjust the condition/logic around the sqrt distance test and the
is_charging_ toggles accordingly.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 98c4a5f3-e29c-42fb-afa8-3473c1023cd3
📒 Files selected for processing (2)
src/mower_simulation/src/SimRobot.cppsrc/mower_simulation/src/SimRobot.h
There was a problem hiding this comment.
Actionable comments posted: 2
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@src/lib/rostime_speed`:
- Line 1: The repository contains a gitlink for the submodule
src/lib/rostime_speed pointing at commit
3b16272bd6b9b3c09a731c0b172f33ce4d4e70d1 but no .gitmodules entry and the commit
is not present in the submodule upstream; add a .gitmodules entry for
src/lib/rostime_speed with the correct submodule URL (matching the upstream
remote used by CI), update the index/working tree if needed (git submodule add
or manually add the entry then git update-index) so the gitlink and .gitmodules
agree, and push the missing commit 3b16272bd6b9b3c09a731c0b172f33ce4d4e70d1 to
the submodule's upstream repository (or point the gitlink to an existing
reachable commit) so the commit is reachable by git-upload-pack before merging.
In `@src/mower_simulation/src/SimRobot.cpp`:
- Around line 136-155: When emergency_latch_ is set the cached noisy twist
values are never updated, so GetTwist and consumers
(DiffDriveService::SendActualTwist, GpsService, ImuService) continue reporting
non-zero motion; fix SimRobot.cpp by updating the emergency branch to reset
last_noisy_vx_ and last_noisy_vr_ to zero (or resample noise centered at 0)
whenever emergency_latch_ is true, ensuring the robot reports a stopped twist
while latched.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: c05f7556-f9b0-4a1c-a291-e60f76b95d6f
📒 Files selected for processing (2)
src/lib/rostime_speedsrc/mower_simulation/src/SimRobot.cpp
There was a problem hiding this comment.
♻️ Duplicate comments (1)
src/mower_simulation/src/SimRobot.cpp (1)
136-155:⚠️ Potential issue | 🟡 MinorEmergency latch freezes cached noisy twist at pre-latch values.
last_noisy_vx_/last_noisy_vr_are only refreshed insideif (!emergency_latch_). Once the latch engages,GetTwistkeeps returning whatever noisy velocity the robot happened to have immediately before the e-stop — often non-zero — indefinitely. Consumers likeDiffDriveService::SendActualTwist,GpsService(ENU motion vector), andImuService(angular rate) will therefore report a moving robot while it is actually latched to a stop. A real robot's encoders/IMU would read ~0 during an e-stop.🛠️ Suggested adjustment
if (!emergency_latch_) { double time_diff_s = (now - last_update_).toSec(); double noisy_vx = vx_ + linear_speed_noise(generator); double noisy_vr = vr_ + angular_speed_noise(generator); last_noisy_vx_ = noisy_vx; last_noisy_vr_ = noisy_vr; ... ＋ } else { ＋ last_noisy_vx_ = 0.0; ＋ last_noisy_vr_ = 0.0; }Kinematics themselves (arc-form with the
1e-6fallback) match the standard ICC model and are consistent with the straight-line limit — those look correct.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/mower_simulation/src/SimRobot.cpp` around lines 136 - 155, When emergency_latch_ is true the code never updates last_noisy_vx_/last_noisy_vr_, causing GetTwist and sensors to keep reporting the pre-latch velocities; fix this by explicitly setting last_noisy_vx_ = 0.0 and last_noisy_vr_ = 0.0 in the branch where emergency_latch_ is true (i.e., the else of the existing if (!emergency_latch_)) so cached noisy twist reflects a stopped robot; update the emergency handling block that surrounds the pose integration logic (references: emergency_latch_, last_noisy_vx_, last_noisy_vr_, GetTwist) to zero out the cached noisy velocities when latched.
🧹 Nitpick comments (2)
src/mower_simulation/src/SimRobot.cpp (2)
34-35: Timer frequency bump looks good; consider a compile-time tie to DiffDriveService.Bumping to 50 Hz and matching
DiffDriveService::tick()cadence is the right call — it keeps integration error small and aligns cached twist with encoder reporting. The "Keep in sync" comment is fine, but a shared constant (header-exported from a common place) would prevent the two periods from drifting apart silently if one side is ever retuned.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/mower_simulation/src/SimRobot.cpp` around lines 34 - 35, The comment warns that the hard-coded 0.02 duration used in nh_.createTimer to set timer_ for SimRobot::SimulationStep can drift from DiffDriveService::tick_schedule_; replace the literal with a shared, header-exported constant (e.g., SIM_TICK_PERIOD or TICK_SCHEDULE) defined in a common header included by both SimRobot and DiffDriveService so both timer_ and DiffDriveService::tick_schedule_ use the same compile-time constant.
158-169: Hysteresis on dock detection is the right fix.0.02 m engage / 0.03 m disengage cleanly prevents chatter from the noisy integrated pose and keeps the charging state machine stable. Using the true pose (
pos_x_/pos_y_) rather than the noisyGetPositionoutput for this check is also the correct choice — the dock contact is a physical event, not an observation.One minor note:
docking_pos_heading_is set bySetDockingPosebut never consulted here. If arriving at the dock from the wrong heading should not count as docked, consider gating on a heading-difference threshold as well; otherwise the stored heading is dead state.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/mower_simulation/src/SimRobot.cpp` around lines 158 - 169, The code correctly uses a distance hysteresis (dock_dist, is_charging_) but ignores the stored docking_pos_heading_ set by SetDockingPose; either use that heading to gate docking or remove the dead field. Fix by adding an angular-check: compute the smallest angular difference between docking_pos_heading_ and the robot's current heading (e.g., pos_theta_ or whatever current heading member is), compare it to a small threshold (e.g., a few degrees in radians) and only set is_charging_ = true when both dock_dist < 0.02 and the heading difference is within the threshold; alternatively, if heading is irrelevant, remove docking_pos_heading_ and any related setters to avoid dead state.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Duplicate comments:
In `@src/mower_simulation/src/SimRobot.cpp`:
- Around line 136-155: When emergency_latch_ is true the code never updates
last_noisy_vx_/last_noisy_vr_, causing GetTwist and sensors to keep reporting
the pre-latch velocities; fix this by explicitly setting last_noisy_vx_ = 0.0
and last_noisy_vr_ = 0.0 in the branch where emergency_latch_ is true (i.e., the
else of the existing if (!emergency_latch_)) so cached noisy twist reflects a
stopped robot; update the emergency handling block that surrounds the pose
integration logic (references: emergency_latch_, last_noisy_vx_, last_noisy_vr_,
GetTwist) to zero out the cached noisy velocities when latched.
---
Nitpick comments:
In `@src/mower_simulation/src/SimRobot.cpp`:
- Around line 34-35: The comment warns that the hard-coded 0.02 duration used in
nh_.createTimer to set timer_ for SimRobot::SimulationStep can drift from
DiffDriveService::tick_schedule_; replace the literal with a shared,
header-exported constant (e.g., SIM_TICK_PERIOD or TICK_SCHEDULE) defined in a
common header included by both SimRobot and DiffDriveService so both timer_ and
DiffDriveService::tick_schedule_ use the same compile-time constant.
- Around line 158-169: The code correctly uses a distance hysteresis (dock_dist,
is_charging_) but ignores the stored docking_pos_heading_ set by SetDockingPose;
either use that heading to gate docking or remove the dead field. Fix by adding
an angular-check: compute the smallest angular difference between
docking_pos_heading_ and the robot's current heading (e.g., pos_theta_ or
whatever current heading member is), compare it to a small threshold (e.g., a
few degrees in radians) and only set is_charging_ = true when both dock_dist <
0.02 and the heading difference is within the threshold; alternatively, if
heading is irrelevant, remove docking_pos_heading_ and any related setters to
avoid dead state.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: ee699d84-c3d3-4b2d-a38f-de45612fdf3e
📒 Files selected for processing (1)
src/mower_simulation/src/SimRobot.cpp
There was a problem hiding this comment.
Actionable comments posted: 1
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Inline comments:
In `@src/mower_simulation/src/SimRobot.cpp`:
- Around line 161-172: The hysteresis is too tight because odometry noise
(noisy_vx/noisy_vr) is integrated even when the robot is commanded to rest, and
pos_x_/pos_y_ are not corrected when is_charging_ latches; update SimRobot.cpp
to prevent drift by either zeroing noisy_vx/noisy_vr when both vx_ and vr_ are
zero OR snapping pos_x_/pos_y_ to docking_pos_x_/docking_pos_y_ immediately when
setting is_charging_ = true (inside the block that sets charging_started_time),
and consider increasing the disengage threshold (dock_dist > 0.03) to a safer
value (e.g., 0.05) if you prefer a margin; make sure changes reference the
existing variables noisy_vx, noisy_vr, vx_, vr_, pos_x_, pos_y_, docking_pos_x_,
docking_pos_y_, and is_charging_.
🪄 Autofix (Beta)
Fix all unresolved CodeRabbit comments on this PR:
- Push a commit to this branch (recommended)
- Create a new PR with the fixes
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: ce40f815-319c-4ffa-88e4-52b2aa6d681d
📒 Files selected for processing (2)
src/lib/rostime_speedsrc/mower_simulation/src/SimRobot.cpp
✅ Files skipped from review due to trivial changes (1)
- src/lib/rostime_speed
There was a problem hiding this comment.
🧹 Nitpick comments (1)
src/mower_simulation/src/SimRobot.cpp (1)
164-180: Hysteresis + snap looks good; consider extracting the thresholds as named constants.The engage/disengage pair plus the pose snap cleanly resolves the drift-across-threshold issue raised earlier. One optional nit: the three related magic numbers (
0.02engage,0.03disengage, and the0.02timer period inStart()) would be easier to reason about as named constants, and would make the relationship between tick period, noise σ, and hysteresis width explicit.♻️ Optional refactor
+// Charging dock hysteresis window (metres). Engage tight, disengage with margin +// so integrated odometry noise during motion cannot flutter the charge state. +static constexpr double kDockEngageDist = 0.02; +static constexpr double kDockDisengageDist = 0.03; @@ - if (!is_charging_ && dock_dist < 0.02) { + if (!is_charging_ && dock_dist < kDockEngageDist) { spdlog::info("Charging"); is_charging_ = true; charging_started_time = ros::Time::now(); // Snap to the exact docking pose to eliminate any pre-latch drift so the // disengage threshold cannot be tripped while the robot sits on the dock. pos_x_ = docking_pos_x_; pos_y_ = docking_pos_y_; - } else if (is_charging_ && dock_dist > 0.03) { + } else if (is_charging_ && dock_dist > kDockDisengageDist) { spdlog::info("Stopped Charging"); is_charging_ = false; }Also worth noting: only
pos_x_/pos_y_are snapped, notpos_heading_. That's fine today becausedock_distis translation-only and the docking procedure lands the robot at the correct heading, but if you later add a heading-based disengage criterion you'll want to snap heading too.🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/mower_simulation/src/SimRobot.cpp` around lines 164 - 180, Extract the magic numbers into named constants and use them in the hysteresis logic: replace the literal 0.02 engage threshold, 0.03 disengage threshold, and the 0.02 timer period referenced in Start() with appropriately named constexprs (e.g., kDockEngageDist, kDockDisengageDist, kDockTickPeriod) and use those symbols in the if-checks and timer setup; also consider snapping pos_heading_ along with pos_x_ and pos_y_ inside the engage branch (use docking_pos_heading_ if available) so the robot's full pose is consistent when charging begins.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Nitpick comments:
In `@src/mower_simulation/src/SimRobot.cpp`:
- Around line 164-180: Extract the magic numbers into named constants and use
them in the hysteresis logic: replace the literal 0.02 engage threshold, 0.03
disengage threshold, and the 0.02 timer period referenced in Start() with
appropriately named constexprs (e.g., kDockEngageDist, kDockDisengageDist,
kDockTickPeriod) and use those symbols in the if-checks and timer setup; also
consider snapping pos_heading_ along with pos_x_ and pos_y_ inside the engage
branch (use docking_pos_heading_ if available) so the robot's full pose is
consistent when charging begins.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 07bceda5-c46b-43b0-892e-1851288fcf8e
📒 Files selected for processing (1)
src/mower_simulation/src/SimRobot.cpp
|
@coderabbitai resume |
✅ Actions performedReviews resumed. |
There was a problem hiding this comment.
🧹 Nitpick comments (1)
src/mower_simulation/src/SimRobot.cpp (1)
144-146: Optional: relax the at-rest check to tolerate tiny numerical residuals.
vx_ == 0.0 && vr_ == 0.0is a strict float equality. Controllers that issue a "stop" typically send exact0.0so this works in practice, but any ε-level residual (e.g., PID wind-down, unit conversions) would reintroduce noise integration and the slow drift the hysteresis window is sized against. A small epsilon would make the guard more defensive without changing behavior for real stop commands.♻️ Suggested tweak
- const bool at_rest = (vx_ == 0.0 && vr_ == 0.0); + constexpr double kRestEps = 1e-6; + const bool at_rest = (std::fabs(vx_) < kRestEps && std::fabs(vr_) < kRestEps); double noisy_vx = at_rest ? 0.0 : vx_ + linear_speed_noise(generator); double noisy_vr = at_rest ? 0.0 : vr_ + angular_speed_noise(generator);🤖 Prompt for AI Agents
Verify each finding against the current code and only fix it if needed. In `@src/mower_simulation/src/SimRobot.cpp` around lines 144 - 146, The at-rest check in SimRobot (variables vx_, vr_, and the computed noisy_vx/noisy_vr) uses strict equality which can miss tiny residuals; change the guard to treat values within a small epsilon as zero (e.g., use fabs(vx_) < epsilon && fabs(vr_) < epsilon) and pick a sensible epsilon (like 1e-6 or a configurable constant such as kAtRestEpsilon) so noise is suppressed for near-zero velocities while preserving behavior for deliberate non-zero commands.
🤖 Prompt for all review comments with AI agents
Verify each finding against the current code and only fix it if needed.
Nitpick comments:
In `@src/mower_simulation/src/SimRobot.cpp`:
- Around line 144-146: The at-rest check in SimRobot (variables vx_, vr_, and
the computed noisy_vx/noisy_vr) uses strict equality which can miss tiny
residuals; change the guard to treat values within a small epsilon as zero
(e.g., use fabs(vx_) < epsilon && fabs(vr_) < epsilon) and pick a sensible
epsilon (like 1e-6 or a configurable constant such as kAtRestEpsilon) so noise
is suppressed for near-zero velocities while preserving behavior for deliberate
non-zero commands.
ℹ️ Review info
⚙️ Run configuration
Configuration used: Organization UI
Review profile: CHILL
Plan: Pro
Run ID: 0eb0a60e-e2aa-420f-af0e-de6fb727ace6
📒 Files selected for processing (1)
src/mower_simulation/src/SimRobot.cpp
1 participant
Summary by CodeRabbit
Bug Fixes
Improvements